#pragma once
#pragma pack(push, 8)
#include <iostream>
#pragma pack(pop)


#include "../SharedData.h"

namespace Driver {
class DriverData {
public:
    explicit DriverData(SlavePDOS *pdos);

public:
    // home guard
    auto IsHomeStarted() const { return (tx_pdos->status_word & 0x0400) == 0x0000; };
    auto IsHomeFinished() const { return (tx_pdos->status_word & 0x0400) == 0x0400; };
    auto IsHomeHalted() const { return (tx_pdos->status_word & 0x0400) == 0x0400; };
    auto IsHomeErrored() const { return (tx_pdos->status_word & 0x2400) == 0x2400; };
    auto IsSwitchToHomeErrored() const { return ((tx_pdos->status_word & 0x2000) == 0x2000) && !IsHomeErrored(); };
    // home action
    void SetHomePara() {
        // TODO
    };
    void StartHome() { rx_pdos->control_word |= 0x0010; };
    void CancelHome() { rx_pdos->control_word &= (~0x0010); };
    void ExitHome() {
        rx_pdos->target_pos = tx_pdos->current_pos;
        rx_pdos->control_mode = 0x0008;
        rx_pdos->control_word &= (~0x0010);
    };
    // operation guard
    bool IsHomeMode() const { return tx_pdos->control_mode_display ==  0x06; };
    bool IsCspMode() const { return tx_pdos->control_mode_display ==  0x08; };
    // operation action
    void ChangeToPPMode() { rx_pdos->control_mode = 0x01; };
    void ChangeToPVMode() { rx_pdos->control_mode = 0x03; };
    void ChangeToPTMode() { rx_pdos->control_mode = 0x04; };
    void ChangeToHomeMode() { rx_pdos->control_mode = 0x06; };
    void ChangeToCspMode() { rx_pdos->control_mode = 0x08; };
    void ChangeToCsvMode() { rx_pdos->control_mode = 0x09; };
    void ChangeToCstMode() { rx_pdos->control_mode = 0x0A; };
    // normal guard
    bool IsReadyToSwitchOn() const {
        return (tx_pdos->status_word & 0x0021) == 0x0021
        &&  (~tx_pdos->status_word & 0x004E) == 0x004E;
    };
    bool IsSwitchedOn() const {
        return (tx_pdos->status_word & 0x0023) == 0x0023
        &&  (~tx_pdos->status_word & 0x004C) == 0x004C;
    };
    bool IsSwitchOnDisabled() const {
        return ((tx_pdos->status_word & 0x0040) == 0x0040)
        &&  (((~tx_pdos->status_word) & 0x000F) == 0x000F);
    };
    bool IsOperationEnabled() const {
        return (tx_pdos->status_word & 0x0027) == 0x0027
        &&  (~tx_pdos->status_word & 0x0048) == 0x0048;
    };
    bool IsQuickStoped() const {
        return (tx_pdos->status_word & 0x0007) == 0x0007
        &&  (~tx_pdos->status_word & 0x0068) == 0x0068;
    };
    bool IsFaultReactionActive() const {
        return (tx_pdos->status_word & 0x000F) == 0x000F
        &&  (~tx_pdos->status_word & 0x0040) == 0x0040;
    };
    // normal action
    void ShutDown() { rx_pdos->control_word = 0x0006; };
    void SwitchOn() { rx_pdos->control_word = 0x0007; };
    void DisableVoltage() { rx_pdos->control_word = 0x0000; };
    void EnableOperation() { rx_pdos->control_word = 0x000F; };
    void SyncPostion() { rx_pdos->target_pos = tx_pdos->current_pos; };
    void DisableOperation() { rx_pdos->control_word = 0x0007; };
    void QuickStop() { rx_pdos->control_word = 0x0002; };
    // TransitionTable guard
    bool IsErrored() const {
        return (tx_pdos->status_word & 0x0008) == 0x0008
        &&  (~tx_pdos->status_word & 0x0040) == 0x0040;
    };
    bool IsNotReadyToSwitchOn() const {
        return (~tx_pdos->status_word & 0x004F) == 0x004F;
    }
    // TransitionTable action
    void Reset() { rx_pdos->control_word = 0x0080; };
    void PrintTxPDO() { std::cout << "error, status_word: " << tx_pdos->status_word << std::endl; }
private:
    RX_PDOS * const rx_pdos;
    const TX_PDOS * const tx_pdos;
};
}
